#include"gdb_server.h"
#include "stm32f405xx.h"
#include <string.h>
#include"user_print.h"
#include "shell.h"
#include "malloc.h"
#include "reg_file.h"
#include "mem_file.h"
#include "thread_file.h"
#include "dap_access.h"
#include "target_flash.h"
volatile unsigned char AckModeFlag=1;
volatile unsigned char GDB_Conection = 0;
void DMA_Uart2Rx_Init(unsigned short* data,unsigned short len)
{

  USART2->CR3 = 3 << 6;
	RCC->AHB1ENR |= 3 << 21;//enable DMA Clock
  DMA1_Stream5->CR = 0;
  DMA1_Stream5->CR = (4 << 25) | (3 << 16) | (1 << 10) | (1 << 8) | (0 << 6) ;
  DMA1_Stream5->M0AR = data;
  DMA1_Stream5->PAR = &(USART2->DR);
  DMA1_Stream5->NDTR = len;
  DMA1->HIFCR |= 0x1D << 16;
  DMA1_Stream5->CR |= 1;
  DMA1->HIFCR |= 0x3D << 6;
}
#define GDB_RX_Len 2048
volatile unsigned int HaltDeBugMode = 0;
static unsigned char GDB_RX_BUF[GDB_RX_Len];
static unsigned int Uart1DMAoldCNTR = GDB_RX_Len;
unsigned int Dump_BinaryPacket(unsigned char*buf,unsigned int len)
{
  unsigned int i;
  user_printf("binaryData:\r\n");
  for(i = 0; i < len; i++)
  {
    user_printf("0x%02x ",buf[i]);
    if((i+1)%10 == 0)
    {
      user_printf("\r\n");
    }
  }
  user_printf("\r\n");
  return 0;
}
unsigned int GetBinaryPacketEscapedLen(unsigned char*buf)
{
  unsigned int i;
  for(i = 0; buf[i] != '#';i++);
  return i;
}
char* BinaryPacketEscaped(unsigned char*buf,unsigned int *EscapeLen,unsigned int PacketEndAddr)
{
  char*Escapebuf;
  unsigned int len,i=0,j=0;
  len = GetBinaryPacketEscapedLen(buf);
  Escapebuf = mymalloc(len);
  if(Escapebuf)
  {
    while((buf[i] != '#') && (i < 4096))
    {
      if(buf[i] == 0x7d)
      {
        Escapebuf[j++]=buf[i+1]^0x20;
        i += 2;
      }
      else
      {
        Escapebuf[j++] = buf[i++];
      }
    }
  }
  *EscapeLen = j;
  return Escapebuf;
}
void DMA_Uart2Send(unsigned short* data,unsigned short len)
{
  DMA1->HIFCR |= 0x3D << 16;
  DMA1_Stream6->CR = 0;
  DMA1_Stream6->M0AR = data;
  DMA1_Stream6->PAR = &(USART2->DR);
  DMA1_Stream6->NDTR = len;
  DMA1_Stream6->CR = (4 << 25) | (3 << 16) | (1 << 10) | (1 << 6) | (0 << 8) | (0 << 6) ;
  DMA1_Stream6->CR |= 1;
  DMA1->HIFCR |= 0x3D << 16;
}
void WaitDmaTransferFinish(void)
{
    while(0u != (DMA1_Stream6->NDTR));
}
unsigned int  Wordlittle2big(unsigned int le) {

    return ((le & 0xffu) << 24u)
            | ((le & 0xff00u) << 8u)
            | ((le & 0xff0000u) >> 8u)
            | ((le >> 24u) & 0xffu);
}
unsigned short  HarfWordlittle2big(unsigned short le) {

    return ((le & 0xffu) << 8u) 
            | ((le & 0xff00u) >> 8u);
}
unsigned int CheckSum(unsigned char *buf,unsigned int len)
{
    unsigned int Sum = 0;
    unsigned int i;
    for(i = 0; i < len; i++)
    {
        Sum += buf[i];
    }
    return (Sum&0xff);
}
#define UART_TC (1 << 6)
void GDB_SendChar(char ch)
{
   while (0 == ((USART2->SR)&UART_TC));
        USART2->DR = ch;
    while (0 == ((USART2->SR)&UART_TC));

}
void packet_write(char*str)
{
    unsigned int len = strlen(str);
    char temp[5];
    if(len == 0)
    {
      if(AckModeFlag)
      {
        // sprintf(sendbuf,"+$#00");
        DMA_Uart2Send("+$#00",5);
        WaitDmaTransferFinish();
      }
      else
      {
        // sprintf(sendbuf,"$#00");
        DMA_Uart2Send("$#00",4);
        WaitDmaTransferFinish();
      }
        
    }
    else
    {
      sprintf(temp,"#%02x",CheckSum(str,len));
      if(AckModeFlag)
      {
        // sendbuf[0] = '+';
        // sprintf(sendbuf+1,"$%s#%02X",str,CheckSum(str,len));
        GDB_SendChar('+');
      }
      GDB_SendChar('$');
      DMA_Uart2Send(str,len);
      WaitDmaTransferFinish();
      DMA_Uart2Send(temp,3);
      WaitDmaTransferFinish();
    }
    
}


void send_thread_xml(void)
{
  packet_write(thread_file);
}
void send_reg_xml(unsigned int offset)
{
  char *sendbuf;
  sendbuf = mymalloc(1056);
  memset(sendbuf,0,1056);
  if((reg_file_len - offset) > 1024)
  {
    memcpy(sendbuf+1,reg_file+offset,1024);
    sendbuf[0] = 'm';
  }
  else
  {
    memcpy(sendbuf+1,reg_file+offset,reg_file_len - offset);
    sendbuf[0] = 'l';
  }
  packet_write(sendbuf);
  myfree(sendbuf);
}
void send_mem_xml(void)
{
#ifdef MEM_XML_GET
  char*buf;
  buf = mem_xml_get();
  packet_write(buf);
  free(buf);
#else
  packet_write(mem_file);
#endif
}
void process_xfer(const char *name, char *args)
{
  const char *mode = args,*args2;
  unsigned int offset,len;
  char filename[10];
  args = strchr(args, ':');
  if(args)
    *args++ = '\0';
  puts(name);
  if (!strcmp(name, "features") && !strcmp(mode, "read"))
  {
    args2 = args;
    args = strchr(args,':');
    if(args)
      *args++ = '\0';
    sscanf(args2,"%s",filename);
    args2 = args;
    args = strchr(args,',');
    if(args)
      *args++ = '\0';
    offset = get_hex(args2);

    args2 = args;
    len = get_hex(args2);
    user_printf("read:%s\toffset:%d,len:%d\r\n",filename,offset,len);
    send_reg_xml(offset);
  }
  if (!strcmp(name, "threads") && !strcmp(mode, "read"))
    send_thread_xml();
  if (!strcmp(name, "memory-map") && !strcmp(mode, "read"))
  {
    send_mem_xml();
  }
  if(!strcmp(name,"exec-file"))
  {
    packet_write("");
  }
}
char* hex2str(char*hex)
{
  unsigned int str_len,i;
  char *str;
  char tempbuf[3]={0};
  str_len = strlen(hex)/2;
  str = (char*)mymalloc(str_len + 1);
  if(str == NULL)
  {
    return str;
  }
  memset(str,0,str_len + 1);
  for(i = 0; i < str_len; i++)
  {
    memcpy(tempbuf,hex + i*2,2);
    sscanf(tempbuf,"%x",str + i);
  }
  str[str_len] = '\r';
  str[str_len + 1] = '\n';
  str[str_len + 2] = '\0';

  return str;
}
char* str2hex(char*str)
{
  unsigned int hex_len,i;
  char *hex;
  hex_len = strlen(str)*2 + 1;
  hex = (char*)mymalloc(hex_len);
  if(hex == NULL)
  {
    return hex;
  }
  memset(hex,0,hex_len);
  for(i = 0; i < strlen(str); i++)
  {
    sprintf(hex+i*2,"%02x",str[i]);
  }
  return hex;
}
void chr2hex(unsigned char ch,unsigned char*out)
{
  unsigned int temp;
  temp = (ch>>4)&0xf;
  if(temp<=9)
  {
    out[0] = '0' + temp;
  }
  else
  {
    out[0] = 'A' + temp - 10;
  }
  temp = (ch>>0)&0xf;
  if(temp<=9)
  {
    out[1] = '0' + temp;
  }
  else
  {
    out[1] = 'A' + temp - 10;
  }
}
unsigned int monitor_output(char*str,unsigned int len)
{
  unsigned int i,ret = 0;
  unsigned char outbuf[16*2+2]="O ";
  memset(outbuf,0,16*2+2);
  outbuf[0] = 'O';
  if(len > 16)
  {
    len = 16;
    ret = 1;
  }
  for(i = 0; i < len; i++)
  {
    chr2hex(str[i],outbuf+i*2+1); 
  }
  packet_write(outbuf);
  return ret;
}
unsigned int monitor_output_string(char*str)
{
  unsigned int len,i;
  len = strlen(str);
  for(i = 0; i < len;)
  {
    if((len - i) > 16)
    {
      monitor_output(str+i,16);
      i+=16;
    }
    else
    {
      monitor_output(str+i,len-i+1);
      i = len;
    }
  }
}
volatile unsigned int monitor_flag = 0;
int monitor_handle(char* args)
{
  char *str,*str2;
  int rtn;
  unsigned int stat,len;
  unsigned int rdata;
  monitor_flag = 1;
  if(*args == '\0')
  {
    char cmd[] = "help\r\n";
    // str = str2hex(" reset_halt :reset and halt cpu\r\nmonitor output finish\r\n");
    // packet_write(str);
    // myfree(str);
    cmd_run(cmd);
    goto close;
  }
  str = hex2str(args);
  if(str != NULL)
  {
      str_std(str);
      cmd_run(str);
      user_printf("%s\r\n",str);
  }
  else
  {
    monitor_output_string("error\r\n");
  }
  
  monitor_flag = 0;
  if(str != NULL)
  {
    myfree(str);
  }
  close:
  packet_write("OK");//finish O packet
  return 0;
}
unsigned int dbg_mem_write_for_M(char *pbuf,unsigned int addr,unsigned int len)
{
  char tempbuf[10];
  unsigned int i,rtn = 0u;
  unsigned int stat;
  unsigned char temp_u8;
  unsigned char *DataBuf;
  
  if((addr >= 0x20000000) && ((addr+len - 1) < 0x20005000))//????????RAM
  {
    DataBuf = mymalloc(len);
    for(i = 0; i < len; i++)
    {
      memcpy(tempbuf,pbuf + i*2,2);
      sscanf(tempbuf,"%zx",&temp_u8);
      DataBuf[i] = temp_u8;
    }
    stat = MemoryWrite(addr,DataBuf,len);
    if(stat != 0)
    {
      rtn = 1;
    }
    myfree(DataBuf);
  }
  else
  {
    rtn = 2;
  }
  
  return rtn;
}
void process_query(char *payload)
{
  const char *name;
  char *args;
  args = strchr(payload, ':');
  if (args)
    *args++ = '\0';
  name = payload;
  if(NULL != strstr(name,"Rcmd"))
  {
    args = strchr(payload, ',');
    if (args)
      *args++ = '\0';
    monitor_handle(args);
  }
  puts(name);
  if(!strcmp(name,"StartNoAckMode"))
  {
    packet_write("OK");
    AckModeFlag = 0;
  }
  if (!strcmp(name, "C"))
  {
    //snprintf(tmpbuf, sizeof(tmpbuf), "QCp%02x.%02x", threads.curr->pid, threads.curr->tid);
    // packet_write(tmpbuf);
    packet_write("QC0");
  }
  if(!strcmp(name,"L"))
  {
    packet_write("qM0100000000000000000");
  }
  if(!strcmp(name,"Rcmd,7265736574"))
  {
    packet_write("OK");
  }
  if (!strcmp(name, "Attached"))
  {
    if(1)// (attach)
      packet_write("1");
    else
      packet_write("0");
  }
  if (!strcmp(name, "Offsets"))
    packet_write("Text=0;Data=0;Bss=0");;
  if (!strcmp(name, "Supported"))
  {
    if(name[-2] == '+')
    {
      AckModeFlag = 1;
    }
    packet_write("PacketSize=800;qXfer:memory-map:read+;qXfer:features:read+;qXfer:threads:read+;QStartNoAckMode+;vContSupported+");
  }
  if (!strcmp(name, "Symbol"))
  {
    packet_write("OK");
    
  }
  if (name == strstr(name, "ThreadExtraInfo"))
  {
    args = payload;
    args = 1 + strchr(args, ',');
    //packet_write("41414141");
    packet_write("");
  }
  if (!strcmp(name, "TStatus"))
    packet_write("");;
  if (!strcmp(name, "Xfer"))
  {
    name = args;
    args = strchr(args, ':');
    *args++ = '\0';
    return process_xfer(name, args);
  }
  if (!strcmp(name, "fThreadInfo"))
  {
    
    packet_write("m0000l");
  }
  if (!strcmp(name, "sThreadInfo"))
  {
    // packet_write("l");
    packet_write("l");
  }
}
volatile DebugTarget DebugObj = {
  .CPU_State = CPU_Halt
};
volatile unsigned char CPU_DetectHalt = 0;
char vpacket_buf[1024];
int vpacket_len = 0;
void process_vpacket(char *payload,unsigned int PacketEndAddr)
{
const char *name;
  char *args;
  int i;
  name = payload;
  puts(name);
  if (NULL != strstr(name,"Cont"))
  {
    args = strchr(payload, ';');
    if (args)
      *args++ = '\0';
    if (!strcmp("Cont?", name))
    {
      // packet_write("T02");
      packet_write("vCont;c;C;s;S");
    }
    else
    {
      if (args[0] == 'c')
      {
        char *Pcmd;
        Pcmd = GDB_RX_BUF + GDB_RX_Len - Uart1DMAoldCNTR;
        DebugObj.CPU_State = CPU_Running;
        ResumeCPU();
      }
      if (args[0] == 's')
      {
        StepiCPU();
        DebugObj.CPU_State = CPU_Halt;
        packet_write("T05");
      }
    }
  }
  args = strchr(payload, ':');
  if (args)
    *args++ = '\0';
  if(!strcmp("FlashErase",name))
  {
    unsigned int addr,temp;
    unsigned int len,stat;
    sscanf(args,"%08x,%08x",&addr,&len);
    user_printf("Erase 0x%x size %d\r\n",addr,len);
    stat = TargetFlashErase(addr,len);
    if(stat == 0)
    {
      packet_write("OK");
    }
    else
    {
      packet_write("E02");
    }
  }
  if(!strcmp("FlashWrite",name))
  {
    unsigned int addr;
    unsigned char *pbuf,*BinBuf,stat;
    unsigned int len;
    sscanf(args,"%08x",&addr);
    pbuf = strchr(args, ':');
    *pbuf++ = 0;
    BinBuf = BinaryPacketEscaped(pbuf,&len,PacketEndAddr);
    if(BinBuf)
    {
      #if 0
      if(len > 16)
      {
        Dump_BinaryPacket(BinBuf,16);
      }
      else
      {
        Dump_BinaryPacket(BinBuf,len);
      }
      
      user_printf("vFlashWrite Addr:0x%08x,len:0x%x\r\n",addr,len);
      #endif
      stat = TargetFlashProgram(addr,BinBuf,len);
      if(stat == 0)
      {
        packet_write("OK");
      }
      else if(stat == 1)
      {
        packet_write("E.memtype"); //for vFlashWrite addressing non-flash memory
      }
      else
      {
        packet_write("E02");
      }
      myfree(BinBuf);
    }
    else
    {
      packet_write("E02");
    }
  }
  if(!strcmp("FlashDone",name))
  {
    packet_write("OK");
  }
  
  if (NULL != strstr(name,"Kill"))
  {
    args = strchr(payload, ';');
    if (args)
      *args++ = '\0';
    packet_write("OK");
    AckModeFlag = 1;
    GDB_Conection = 0;
  }
  if (!strcmp("MustReplyEmpty", name))
  {
    packet_write("");
  }
  if (name == strstr(name, "File:"))
  {
    char *operation = strchr(name, ':') + 1;
    if (operation == strstr(operation, "open:"))
    {
      
    }
    else if (operation == strstr(operation, "close:"))
    {
      char *parameter = strchr(operation, ':') + 1;
      size_t fd;
    //   assert(sscanf(parameter, "%zx", &fd) == 1);
      close(fd);
      packet_write("F0");
    }
    else if (operation == strstr(operation, "pread:"))
    {
      
    }
    else if (operation == strstr(operation, "setfs:"))
    {
      
      packet_write("F0");
    }
    else
      packet_write("");;
  }
}
// #define packet_debug
unsigned int process_packet(char *recvbuf)
{
  char *cmd;
  char request ;
  char *payload ;
  char *checksum;
  unsigned int payload_len;
  unsigned int PacketEndAddr;
  #ifdef packet_debug
  if((NULL == strstr(recvbuf,"$vFlashWrite:"))&&(NULL == strstr(recvbuf,"$X")))
  {
    user_printf("-----------------------------------:\r\n");
    user_printf("process_packet:\r\n");
    user_printf("%s\r\n",recvbuf);
    user_printf("********************************:\r\n");
  }
  #endif
  cmd = strstr(recvbuf,"$");
  if(cmd == NULL)
  {
    //shell cmd
    unsigned int i;
    for(i = 0; recvbuf[i] != 0; i++)
    {
      if(recvbuf[i]=='#')
      {
        recvbuf[i]='\r';
        recvbuf[i+1]='\n';
        recvbuf[i+2]='\n';
        break;
      }
    }
    str_std(recvbuf);
    cmd_run(recvbuf);
    return 0;
  }
  
  
  checksum = strstr(recvbuf,"#");
  if(checksum != NULL)
  {
    unsigned char packet_checksum;
    //$ payloadData #
    PacketEndAddr = (unsigned int)checksum;
    payload_len = checksum - cmd - 1;
    if((cmd[1] == 'v')&&((cmd+1) == strstr(cmd,"vFlashWrite")))
    {

    }
    else
    {
      checksum[0] = 0;
    }
    checksum++;
    checksum[2] = 0;
    packet_checksum = get_hex(checksum);
    if(GDB_Conection == 0)
    {
      if(packet_checksum == CheckSum(cmd+1,payload_len))
      {
        GDB_SendChar('+');
        GDB_Conection = 1;
        user_printf("GDB_Connectd..\r\n");
      }
    }
  }
  if(HaltDeBugMode == 0)
  {
    HaltDeBugMode = 1;
    haltdebug();
    DebugInit();

  }
  // puts(cmd);
  request = cmd[1];
  payload = (char *)&cmd[2];
  switch (request)
  {
  case 'D':
    packet_write("OK");
    AckModeFlag = 1;
    GDB_Conection = 0;
    return 1;
  case 'g'://打印寄存�??
  {
    char *membuf;
    unsigned int i,Data;
    unsigned int stat;
    membuf = mymalloc(1024);
    for(i=0; i<=15; i++)
    {
      // if(i==15)
      // {
      //   sprintf(membuf + 8*i,"%08x",Wordlittle2big(pc));
      // }
      // else
      // {
      //   sprintf(membuf + 8*i,"%08x",Wordlittle2big(i<<2));
      // }
      stat = CPU_RegRead(i,&Data);
      if(stat == 0)
      {
        sprintf(membuf + 8*i,"%08x",Wordlittle2big(Data));
      }
      else
      {
        #if 0
        user_printf("reg[%d] read error\r\n",i);
        #endif
      }
    }
    packet_write(membuf);
    myfree(membuf);
    break;
  }
  case 'H':
    if ('g' == *payload++)
    {
    //   pid_t tid;
    //   char *dot = strchr(payload, '.');
    //   assert(dot);
    //   tid = strtol(dot, NULL, 16);
    //   if (tid > 0)
    //     set_curr_thread(tid);
    packet_write("OK");
    }
    else if('c'== *payload++)
    {
      packet_write("OK");
    }
    else
    {
      packet_write("");
    }
    
    break;
  case 'm':
  {
    unsigned int maddr, mlen,i, mdata,stat;
    char *membuf;
    unsigned char*databuf;
    if( sscanf(payload, "%x,%x", &maddr, &mlen) == 2)
    {
      #if 0
        user_printf("mem read addr:0x%08x size:%d\r\n",maddr,mlen);
      #endif
        switch (mlen)
        {
        case 1:
          membuf = mymalloc(32);
          stat = MemoryRead(maddr,&mdata,1);
          if(stat != 0)
          {
            user_printf("m byte error\r\n");
          }
          sprintf(membuf,"%02X",mdata&0xff);
          packet_write(membuf);
          myfree(membuf);
          break;
        case 2:
          membuf = mymalloc(32);
          stat = MemoryRead(maddr,&mdata,2);
          if(stat != 0)
          {
            user_printf("m halfword error\r\n");
          }
          sprintf(membuf,"%02X%02X",mdata&0xff,(mdata >> 8)&0xff);
          packet_write(membuf);
          myfree(membuf);
          break;
        case 4:
          membuf = mymalloc(32);
          stat = MemoryRead(maddr,&mdata,4);
          if(stat != 0)
          {
            user_printf("m word error\r\n");
          }
          sprintf(membuf,"%02X%02X%02X%02X",mdata&0xff,(mdata >> 8)&0xff,(mdata >> 16)&0xff,(mdata >> 24)&0xff);
          packet_write(membuf);
          myfree(membuf);
          // packet_write("01020304");
          break;
        default:
          membuf = mymalloc(mlen*2);
          if(membuf)
          {
            memset(membuf,0,mlen*2);
          }
          databuf = mymalloc(mlen);
          stat = MemoryRead(maddr,databuf,mlen);
          if(stat != 0)
          {
            user_printf("m %d error\r\n",mlen);
          }
          #if 0
          Dump_BinaryPacket(databuf,mlen);
          #endif
          for(i=0; i < mlen;i++)
          {
            sprintf(membuf + i*2,"%02X",databuf[i]&0xff);
          }
          if(membuf)
          {
            packet_write(membuf);
          }
          else
          {
            packet_write("E02");
          }
          if(membuf)
          {
            myfree(membuf);
          }
          if(databuf)
          {
            myfree(databuf);
          }
          break;
        }
    }
    else
    {
        user_printf("mem read error\r\n");
        packet_write("OK");
    }
    
    
    break;
  }
  case 'M':
  {
    unsigned int maddr, mlen, mdata,stat;
    if(sscanf(payload, "%x,%x:", &maddr, &mlen) == 2)
    {
      #if 0
       user_printf("write mem addr:0x%08x size:%d\r\n",maddr,mlen);
       #endif
       if(0 == dbg_mem_write_for_M(strchr(payload,':') + 1,maddr,mlen))
        {
          packet_write("OK");
        }
        else
        {
          packet_write("E02");
        }
      
    }
    else
    {
        user_printf("mem write error\r\n");
        packet_write("E04");
    }
    
    break;
  }
  case 'X':
  {
    //load程序
    unsigned int maddr, mlen,stat;
    unsigned char *Data,*EscapedBin;
    if(sscanf(payload, "%x,%x:", &maddr, &mlen) == 2)
    {
      Data = strchr(payload,':');
      Data++;
      if(mlen > 0)
      {
        EscapedBin = BinaryPacketEscaped(Data,&mlen,PacketEndAddr);
      }
      user_printf("X addr:0x%08x len:%d\r\n",maddr,mlen);
      if(mlen>0)
      {
        if(EscapedBin)
        {
          Dump_BinaryPacket(EscapedBin,mlen);
          if(0 == IsFlash(maddr,mlen))
          {
            stat = MemoryWrite(maddr,EscapedBin,mlen);
            if(stat != 0)
            {
              packet_write("E02");
            }
            else
            {
              packet_write("OK");
            }
          }
          else
          {
            stat = TargetFlashWrite(maddr,EscapedBin,mlen);
            if(stat != 0)
            {
              packet_write("E02");
            }
            else
            {
              packet_write("OK");
            }
          }
          myfree(EscapedBin);
        }
        else
        {
          packet_write("E02");
        }
      }
      else
      {
        packet_write("OK");
      }
    }
    break;
  }
  case 'p':
  {
    //读取寄存器�?
    char *reg_index;
    char *tempbuf;
    unsigned int read_reg_value,stat;
    unsigned int cpu_reg_index=0,reg_addr;
    reg_index = payload;
    sscanf(reg_index,"%x",&cpu_reg_index);
    reg_addr = cpu_reg_index;
    if(cpu_reg_index > CM3_PSP)
    {
      reg_addr = CM3_Special;
    }
    stat = CPU_RegRead(reg_addr,&read_reg_value);
    if(stat == 0)
    {
      if(cpu_reg_index != cpu_reg_index)
      {
        if(cpu_reg_index == 19)
        {
          read_reg_value = read_reg_value & 0xff;
        }
        else if(cpu_reg_index == 20)
        {
          read_reg_value = (read_reg_value >> 8) & 0xff;
        }
        else if(cpu_reg_index == 21)
        {
          read_reg_value = (read_reg_value >> 16) & 0xff;
        }
        else if(cpu_reg_index == 22)
        {
          read_reg_value = (read_reg_value >> 24) & 0xff;
        }
      }
      user_printf("reg[%s] = %08x\r\n",reg_index,read_reg_value);
    }
    else
    {
      user_printf("reg[%s] = %08x read error\r\n",reg_index);
    }
    read_reg_value = Wordlittle2big(read_reg_value);
    tempbuf = mymalloc(32);
    sprintf(tempbuf,"%08x",read_reg_value);
    packet_write(tempbuf);
    myfree(tempbuf);
    break;
  }
  case 'P':
  {
    //写寄存器�??
    char *reg_value;
    char *reg_index;
    unsigned int write_value,Data,stat;
    unsigned int reg_addr;
    unsigned int reg_addr_real;
    reg_index = payload;
    sscanf(payload,"%x=",&reg_addr);
    reg_value = strchr(payload,'=');
    if(reg_value != NULL)
    {
      *reg_value = 0;
      reg_value++;
      reg_addr_real = reg_addr;
      sscanf(reg_value,"%08x",&write_value);
      write_value = Wordlittle2big(write_value);
      user_printf("reg %s write %08x\r\n",reg_index,write_value);
      if(reg_addr > CM3_PSP)
      {
        reg_addr_real = CM3_Special;
        stat = CPU_RegRead(reg_addr_real,&Data);
        if(stat !=0 )
        {
          user_printf("P packet Read error\r\n");
        }
        else
        {
          if(reg_addr_real == 19)
          {
            Data = Data & (~0xff);
            write_value = Data | write_value;
          }
          else if(reg_addr_real == 20)
          {
            Data = Data & (~(0xff << 8));
            write_value = Data | (write_value << 8);
          }
          else if(reg_addr_real == 21)
          {
            Data = Data & (~(0xff << 16));
            write_value = Data | (write_value << 16);
          }
          else if(reg_addr_real == 22)
          {
            Data = Data & (~(0xff << 24));
            write_value = Data | (write_value << 24);
          }
        }
      }
      stat = CPU_RegWrite(reg_addr_real,write_value);
      if(stat != 0)
      {
        user_printf("cpu write error \r\n");
      }
    }
    packet_write("OK");
    break;
  }
  case 'Q':
  case 'q':
    process_query(payload);
    break;
  case 'v':
    process_vpacket(payload,PacketEndAddr);
    break;
  case 'Z':
  {
    unsigned int type, addr, length,stat;
    if(sscanf(payload, "%x,%x,%x", &type, &addr, &length) == 3)
    {
      user_printf("z break type: %d addr:0x%08x length:%d \r\n",type,addr,length);
      stat = InsertBreakpointOrWatchpoint(type,addr,length);
      if(stat == 0)
      {
        packet_write("OK");
      }
      else
      {
        packet_write("E02");
      }
    }
    else
    {
      packet_write("E02");
    }
    //添加断点
    break;
  }
  case 'z':
  {
    size_t type, addr, length,stat;
    if(sscanf(payload, "%x,%x,%x", &type, &addr, &length) == 3)
    {
      user_printf("Z break type: %d addr:0x%08x length:%d \r\n",type,addr,length);
      stat = RemovetBreakpointOrWatchpoint(type,addr,length);
      if(stat == 0)
      {
        packet_write("OK");
      }
      else
      {
        packet_write("E02");
      }
    }
    else
    {
      packet_write("OK");
      //取消断点
    }
    break;
  }
  case '?':
    packet_write("S05");
    break;
    case '!':
        packet_write("OK");
        break;
  default:
    packet_write("");;
  }
  return 0;
}

#define GDB_Buf2Len 4096
unsigned char GDB_Buf2[4096];
unsigned int GDB_BufRxLen =0;
unsigned int GDB_Recv(char*buf)
{
    unsigned char ch,ch1=0,ch2=0,ch3=0;
    unsigned int temp,stat;
    unsigned int break_flag = 0;
    unsigned int TransferCount,i,j,DataPoint=0;
    do{
        if(CPU_DetectHalt)
        {
          CPU_DetectHalt = 0;
          packet_write("T05");
        }
        if(Uart1DMAoldCNTR != (DMA1_Stream5->NDTR))
        {
            temp = DMA1_Stream5->NDTR;
            if(temp < Uart1DMAoldCNTR)
            {
              TransferCount = Uart1DMAoldCNTR - temp;
                
            }
            else
            {
              TransferCount = (GDB_RX_Len - temp) + Uart1DMAoldCNTR ;
            }
            // Uart1DMAoldCNTR = temp;
            j = GDB_RX_Len - Uart1DMAoldCNTR;
            for(i = 0; i < TransferCount; i++)
            {
                if((i + j) < GDB_RX_Len)
                {
                    ch = GDB_RX_BUF[i + j];
                }
                else
                {
                    ch = GDB_RX_BUF[i + j - GDB_RX_Len];
                }
                if((ch == 3)&&(DebugObj.CPU_State == CPU_Running))
                {
                    __asm("cpsid i");
                    DebugObj.CPU_State == CPU_Halt;
                    __asm("cpsie i");
                    stat = HaltCPU();
                    if(stat == 0)
                    {
                      DebugObj.CPU_State = CPU_Halt;
                      packet_write("T02");
                    }
                    else
                    {
                      user_printf("halted cpu error\r\n");
                      packet_write("E 02");
                    }
                    // cpu_run = 0;
                    // continue;
                }
                buf[DataPoint] = ch;
                DataPoint++;
                ch3 = ch2;
                ch2 = ch1;
                ch1 = ch;
                if(ch3 == '#')
                {
                    if(Uart1DMAoldCNTR > i)
                    {
                        Uart1DMAoldCNTR -= i;
                        Uart1DMAoldCNTR--;
                    }
                    else
                    {
                        Uart1DMAoldCNTR = GDB_RX_Len - ( (i + 1) - Uart1DMAoldCNTR);
                    }
                    break_flag = 1;
                    break;
                }
            }
            if(break_flag == 0)
            {
                Uart1DMAoldCNTR = temp;
            }
        }
    }while (break_flag == 0);
    return DataPoint;
}
void Uart_GDB_ServerStart(void)
{
    SWD_Init();
    DP_Write(DAP_ABORT_WR,0x1f);
    DP_Write(DAP_CTRLSTAT_WR,0x50000000);//System power-up request Debug power-up request
    DP_Write(DAP_ABORT_WR,0x1f);//
    DMA_Uart2Rx_Init(GDB_RX_BUF,GDB_RX_Len);
    Uart1DMAoldCNTR =  DMA1_Stream5->NDTR;
    while(1)
    {
        GDB_BufRxLen = GDB_Recv(GDB_Buf2);
        process_packet(GDB_Buf2);
        memset(GDB_Buf2,0,GDB_BufRxLen);
    }
}